import time
from car_base.constant import *
from tools.log import log
import threading
import multiprocessing as mp
import numpy as np

class DataReadThread(threading.Thread):
    def __init__(self, client, stopFlag:mp.Value):
        """
        无人车底层数据接收线程
        :param client: wifi/usart的对象 用于获取datRead函数
        :param stopFlag: 无人车底层程序状态反馈
        """
        threading.Thread.__init__(self)
        self.stopFlag = stopFlag
        self.client = client
        self.flag = True

    def setFlag(self, flag:bool):
        self.flag = flag

    def run(self):
        while self.flag:
            time.sleep(0.2)
            dat_msg = self.client.datRead()
            # 获取小车反馈的状态标志
            self.stopFlag.value = dat_msg[2]

class DataSendThread(threading.Thread):
    def __init__(self, client, q_send:mp.Queue):
        """
        无人车控制指令发送线程
        :param client:无人车底层数据接收线程
        :param q_send:无人车底层程序状态反馈
        """
        threading.Thread.__init__(self)
        self.q_send = q_send
        self.client = client
        self.flag = True

    def setFlag(self, flag:bool):
        self.flag = flag

    def run(self):
        # 获取消息队列并发送
        while self.flag:
            if self.q_send.empty():
                continue
            else:
                dat = self.q_send.get()
                log.info(dat)
                self.client.send(dat)

def carDataThreadRun(client, stopFlag:mp.Value, q_send:mp.Queue):
    try:
        read_thread = DataReadThread(client, stopFlag)
        send_thread = DataSendThread(client, q_send)
        read_thread.start()
        send_thread.start()
        log.info("无人车数据接收和发送线程启动成功！！")
    except:
        log.error("无人车数据接收和发送线程启动失败!!")


class CarDrive():
    def __init__(self, stopFlag:mp.Value, q_send:mp.Queue, with_flag=True):
        """
        无人车运动控制及沙盘标志物控制函数
        :param q_send: 用于传达发送控制数据（此消息队列通过线程的方式自动发送）
        :param stopFlag: 多进程共享变量（通过线程更新参数）
        :param with_flag: 是否开启
        """
        self.with_flag = with_flag
        self.speed = 80
        self.car_dev = MAIN_CAR
        self.q_send = q_send
        self.stopFlag = stopFlag

    def datSend(self, comm0, comm1, comm2, comm3, comm4):
        send_dat = np.zeros((8,), np.uint8)
        send_dat[0] = 0x55
        send_dat[1] = comm0
        send_dat[2] = comm1
        send_dat[3] = comm2
        send_dat[4] = comm3
        send_dat[5] = comm4
        send_dat[6] = (send_dat[2] + send_dat[3] + send_dat[4] + send_dat[5]) % 256
        send_dat[7] = 0xBB
        if not self.q_send.full():
            self.q_send.put(send_dat)

    def _withFlag(self, value):
        log.info(value)
        while self.with_flag:
            if(self.stopFlag.value == value):
                break

    """
    小车基本运动控制：停止、前进、后退、左转、右转、循迹
    """
    def stop(self):
        # 停止
        self.datSend(self.car_dev, 0x01, 0, 0, 0)
        self._withFlag(0x00)

    def go(self, speed=80, mp=400):
        # 前进
        if self.with_flag:
            self.stop()
        self.datSend(self.car_dev, 0x02, speed, mp % 256, mp >> 8)
        self._withFlag(0x03)

    def back(self, speed=80, mp=400):
        # 后退
        if self.with_flag:
            self.stop()
        self.datSend(self.car_dev, 0x03, speed, mp % 256, mp >> 8)
        self._withFlag(0x03)

    def left(self, speed=80):
        # 左转
        if self.with_flag:
            self.stop()
        self.datSend(self.car_dev, 0x04, speed, 0, 0)
        self._withFlag(0x02)

    def right(self, speed=80):
        # 右转
        if self.with_flag:
            self.stop()
        self.datSend(self.car_dev, 0x05, speed, 0, 0)
        self._withFlag(0x02)

    def track(self, speed=80):
        # 循迹
        if self.with_flag:
            self.stop()
        self.datSend(self.car_dev, 0x06, speed, 0, 0)
        self._withFlag(0x01)

    def buzzer(self, first):
        # 蜂鸣器 0x01打开 0x00关闭
        self.datSend(self.car_dev, BUZZER, first, 0, 0)

    def light(self, left, right):
        # 左右转向灯
        self.datSend(self.car_dev, INDICATOR_LIGHT, left, right, 0)

    """
    沙盘标志物控制  
    """
    def gateCon(self, value):
        # 道闸开启(0x01)或关闭(0x00)
        self.datSend(ROAD_GATE, 0x01, value, 0, 0)
        time.sleep(1)

    def gatePlate(self, plt=""):
        # 道闸显示车牌信息
        if len(plt) != 6:
            log.info("--plate_len_err!!")
        else:
            plate = list(map(ord, plt))
            a, b, c, d, e, f = plate
            self.datSend(ROAD_GATE, 0x10, a, b, c)
            time.sleep(0.3)
            self.datSend(ROAD_GATE, 0x11, d, e, f)
            time.sleep(2)

    def tftDis(self, value):
        # TFT显示标志物
        self.datSend(TFT_SHOW_A, 0x10, value, 0, 0)
        time.sleep(0.3)

    def openInfrared(self, codeNarray):
        # 打开红外报警器
        if len(codeNarray) == 6:
            a, b, c, d, e, f, = codeNarray
            self.datSend(self.car_dev, INFRARED_FORWARD_3, a, b, c)
            time.sleep(0.5)
            self.datSend(self.car_dev, INFRARED_BACK_3, d, e, f)
            time.sleep(0.5)
            self.datSend(self.car_dev, INFRARED_SEND, 0, 0, 0)
            time.sleep(1)
        else:
            print("Alarm code lenght not equal to six")

    def lightSource(self, gear):
        # 控制光源挡位
        MAJOR = 0x00
        if gear == LandmarkMode.GEAR_PLUS_ONE:
            MAJOR = BRIGHTNESS_PLUS_ONE
        elif gear == LandmarkMode.GEAR_PLUS_TWO:
            MAJOR = BRIGHTNESS_PLUS_TWO
        elif gear == LandmarkMode.GEAR_PLUS_THRID:
            MAJOR = BRIGHTNESS_PLUS_THREE

        self.datSend(self.car_dev, MAJOR, 0, 0, 0)

    def ledResetTime(self):
        # LED开始计时
        self.datSend(LED_TIME, 0x01, 0, 0, 0)

    def ledCleanTime(self):
        # LED 计时清零
        self.datSend(LED_TIME, 0x02, 0, 0, 0)

    def ledStopTime(self):
        # LED暂停计时
        self.datSend(LED_TIME, 0x00, 0, 0, 0)

    def ledTimeMode(self, timeMode: LandmarkMode):
        # LED计时模式切换
        if timeMode == LandmarkMode.TIMER_START:
            # LED开始计时
            self.ledResetTime()
        elif timeMode == LandmarkMode.TIMER_STOP:
            # LED 暂停计时
            self.ledStopTime()
        elif timeMode == LandmarkMode.TIMER_CLEAN:
            # LED 计时清零
            self.ledCleanTime()

    def garageEvator(self, garageType: LandmarkMode, layerNum: int):
        # 立体车库指定到某一层
        if not layerNum in range(1, 5):
            log.error("layerNum not in [1,4]")
        if garageType == LandmarkMode.MODE_A:
            self.datSend(GARAGE_A, GARAGE_EVATOR, layerNum, 0, 0)
        elif garageType == LandmarkMode.MODE_B:
            self.datSend(GARAGE_B, GARAGE_EVATOR, layerNum, 0, 0)

    def getGarageFloorNum(self, garageType: LandmarkMode):
        # 获取车库停留在那一层
        if garageType == LandmarkMode.MODE_A:
            self.datSend(GARAGE_A, GARAGE_STATUS, 0x01, 0, 0)
        elif garageType == LandmarkMode.MODE_B:
            self.datSend(GARAGE_B, GARAGE_STATUS, 0x01, 0, 0)

    # def garageGoLayer_Auto(self, garageType: LandmarkMode, level: GarageLayer):
    #     # 到达车库A/B指定的层数
    #     MODE = GARAGE_A
    #     if garageType == LandmarkMode.MODE_A:
    #         MODE = GARAGE_A
    #     elif garageType == LandmarkMode.MODE_B:
    #         MODE = GARAGE_B
    #
    #     # 发送指令，控制车库到达指定楼层
    #     self.garageEvator(MODE, GARAGE_EVATOR, level.value)
    #     time.sleep(3)
    #     self.getGarageFloorNum(MODE)
    #     time.sleep(1)
    #
    #     # 等待车库到达指定楼层  阻塞函数
    #     i = 0
    #     while True:
    #         log.info("运行状态：{}".format(self.landMarkFlag.value))
    #         if self.landMarkFlag.value == (0x08 + level.value):
    #             log.info("garageGoLayer_Auto is success now at：{} layer".format(level.value))
    #             return True
    #         time.sleep(0.1)
    #         i += 1
    #         if i % 5 == 0:
    #             # 获取当前车库楼层
    #             self.__buildGarageelPack(MODE, GARAGE_STATUS, 0x01)
    #             time.sleep(0.2)
    #         if i % 15 == 0:
    #             # 发送指令，控制车库到达指定楼层
    #             self.__buildGarageelPack(MODE, GARAGE_EVATOR, level.value)
    #             time.sleep(0.2)
    #         if i > 500:
    #             log.error("The garage is abnormal, which is most "
    #                       "likely caused by the fault of the garage "
    #                       "Zigbee module, please plug in the module again")
    #             return False
